#include "bsp_itr.h"

int Pin_ITR[4] = {FRONT_ITR,BEHIND_ITR,LEFT_ITR,RIGHT_ITR};
char *Mode_ITR_char[] = {"Front_ITR:", "       Behind_ITR:", "       Left_ITR:", "       Right_ITR:"};
uint8_t Mode_ITR[4] = {0}; 

void ITR_Init()
{
  pinMode(FRONT_ITR,INPUT);
  pinMode(BEHIND_ITR,INPUT);
  pinMode(LEFT_ITR,INPUT);
  pinMode(RIGHT_ITR,INPUT);
}

void getITR(uint8_t* pmem, int choice)
{
  selectITR(choice);

  for (int i = 0; i < 4; i++)
  {
    if (Mode_ITR[i])
    {
      Serial.print(*(Mode_ITR_char + i));
      Serial.print(digitalRead(Pin_ITR[i]));
    }
  }
  Serial.println();
}

void printITR()
{
  for (int i = 0; i < 4; i++)
  {
      Serial.print(*(Mode_ITR_char + i));
      Serial.print(analogRead(FRONT_ITR + i));
  }
}

void selectGray(int choice)
{

  Mode_ITR[0] = (choice & 0b1000) >> 3;
  Mode_ITR[1] = (choice & 0b0100) >> 2;
  Mode_ITR[2] = (choice & 0b0010) >> 1;
  Mode_ITR[3] = (choice & 0b0001);

}
